
/** Simulation.cpp
 * 4 legged Robot simulation with PID Controlled joints
 */

#include <openrave-core.h>
#include <vector>
#include <iostream>

#include "orexample.h"
#include "pidcontroller.hpp"

// If we follow the google c++ standard, 'using namespace' is forbidden
// http://google-styleguide.googlecode.com/svn/trunk/cppguide.xml#Namespaces
using std::cout;
using std::endl;
using std::vector;
using std::string;

namespace cppexamples {

class ShowSensorsExample : public OpenRAVEExample {
public:
    virtual void demothread(int argc, char ** argv) {
/*
        penv->Load("data/testwamcamera.env.xml");
        size_t ienablesensor = 0;
        // get all the sensors, this includes all attached robot sensors
        vector<SensorBasePtr> sensors;
        penv->GetSensors(sensors);
        while(IsOk()) {
            for(size_t isensor = 0; isensor < sensors.size(); ++isensor) {
                sensors[isensor]->Configure(isensor == ienablesensor ? SensorBase::CC_PowerOn : SensorBase::CC_PowerOff);
                sensors[isensor]->Configure(isensor == ienablesensor ? SensorBase::CC_RenderDataOn : SensorBase::CC_RenderDataOff);
            }
            ienablesensor = (ienablesensor+1)%sensors.size();
            boost::this_thread::sleep(boost::posix_time::seconds(5));
        }
*/
        penv->Load("test.env.xml");

        // attach a physics engine
        penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode"));
        penv->GetPhysicsEngine()->SetGravity(OpenRAVE::Vector(0,0,-9.8));

		// Find all the robots and get a pointer to them
        vector<OpenRAVE::RobotBasePtr> vrobots;
        penv->GetRobots(vrobots);
        OpenRAVE::RobotBasePtr probot = vrobots.at(0);




        vector<int> jointIndex;
        OpenRAVE::ControllerBasePtr jointController;


		{

			OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
/*
            MultiControllerPtr multi(new MultiController(penv));
            vector<int> dofindices(probot->GetDOF());
            for(int i = 0; i < probot->GetDOF(); ++i) {
                dofindices[i] = i;
            }
            probot->SetController(multi,dofindices,1); // control everything
*/

jointController = RaveCreateController(penv,"odevelocity");
            vector<int> dofindices(probot->GetDOF());
            for(int i = 0; i < probot->GetDOF(); ++i) {
                dofindices[i] = i;
            }
			probot->SetController(jointController, dofindices,1) ;



            // set the velocity controller on all joints that have 'wheel' in their description
            for(vector<OpenRAVE::KinBody::JointPtr>::const_iterator itjoint = probot->GetJoints().begin(); itjoint != probot->GetJoints().end(); ++itjoint) {
				cout << "Got joint: " << (*itjoint)->GetName() << " - ";
	                if( (*itjoint)->GetName().find("j") != string::npos ) {
		                for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
							cout << "DOFIndex: " << (*itjoint)->GetDOFIndex()+i << endl;
							jointIndex.push_back((*itjoint)->GetDOFIndex()+i);
		                }
                	}
            }

			// Attach controllers to the joints
			cout << "Attaching controllers.\n";
			cout << "jointIndex size: " << jointIndex.size() << endl;
 /*           if(jointIndex.size() > 0 ) {
				jointController = RaveCreateController(penv,"odevelocity");
				multi->AttachController(jointController, jointIndex, 0);
            }
*/


		}

			vector<PIDController*> control;
			for (unsigned int i=0; i<jointIndex.size(); i++) {
				PIDController* tmp = new PIDController();
				control.push_back(tmp);

			}

			vector<OpenRAVE::dReal> tau;
			vector<OpenRAVE::dReal> q;
			vector<OpenRAVE::dReal> q_dot;
			vector<OpenRAVE::dReal> q_d;
			vector<OpenRAVE::dReal> q_dot_d;

			tau.resize(jointIndex.size());
			q.resize(jointIndex.size());
			q_dot.resize(jointIndex.size());
			q_d.resize(jointIndex.size());
			q_dot_d.resize(jointIndex.size());




//			q_d[0] = -0.7;
//			q_d[1] = 2.4;
//			q_d[2] = -2.6;
//			q_d[0] = -45*M_PI/180;
//			q_d[1] = 90*M_PI/180;
//			q_d[2] = -90*M_PI/180;

			double j[3];
			j[0] = -60*M_PI/180;
			j[1] = 90*M_PI/180;
			j[2] = -90*M_PI/180;

			double K[3];
			K[0] = 1000.0;
			K[1] = 30.0;		//2*sqrt(Kp);
			K[2] = 0.005;
		
			for (unsigned int i=0; i<jointIndex.size(); i++) {
				//int tmp = i%3;
				//cout << tmp << endl;



				q_d[i] = j[i%3];
				q_dot_d[i] = 0;
				cout << "joint: " << i << " Kp: " << K[0] << ", Kd: " << K[1] << ", Ki: " << K[2] << endl;
				control[i]->setKp(K[0]);
				control[i]->setKd(K[1]);
				control[i]->setKi(K[2]);
			}

/*			
			vector<KinBody::LinkPtr> links(probot->GetLinks());
			for (unsigned int i=0; i<links.size(); i++) {
				cout << i << ": " << &links.GetMass() << endl;
					
			}
*/
/*
			q_d[0] = -70*M_PI/180;
			q_d[1] = 90*M_PI/180;
			q_d[2] = -110*M_PI/180;


			q_dot_d[0] = 0;
			q_dot_d[1] = 0;
			q_dot_d[2] = 0;
*/


		cout << "Entering loop\n";

    //OpenRAVE::dReal pTerm;
    //OpenRAVE::dReal dTerm;
    //OpenRAVE::dReal iTerm[3];


		float old_time;

		//double intError;

		cout << "Aiming for positions: " << q_d[0] << ":" << q_d[1] << ":" << q_d[2] << "\n";

		cout << "SimTime:Joint0:Pos:vel:torque:Joint1:Pos:vel:torque:Joint2:Pos:vel:torque\n";

		while(IsOk()) {

			OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
			
                if( !!jointController ) {
/*
                   stringstream sout,ss; ss << "setvelocity ";
                    for(size_t i = 0; i < jointIndex.size(); ++i) {
                        //ss << 2*(RaveRandomFloat()-0.5) << " ";
						ss << 1 << " ";
                    }
                    if( !jointController->SendCommand(sout,ss) ) {
                        RAVELOG_WARN("failed to send velocity command\n");
                    }
*/

	       	float simtime = penv->GetSimulationTime()*1e-6;
			if ((simtime - old_time) > 0) {
        		//cout << "sim time: " << simtime << ": ";
				cout << simtime;
			}	


			// Get Joint position and velocities
			probot->GetDOFValues(q);
			probot->GetDOFVelocities(q_dot);

			for (unsigned int i=0; i<jointIndex.size(); i++) {
				double pError = q[i] - q_d[i];
				double vError = q_dot[i] - q_dot_d[i];
				control[i]->setPosError(pError);
				control[i]->setVelError(vError);
				control[i]->updateController();
				tau[i] = control[i]->getOutput();

				if ((simtime - old_time) > 0) {
					//cout << "Joint:" << i << ":Pos:" << q[i] << ":Vel:" << q_dot[i] << ":Torque:" << tau[i] << ":";	//<< endl;
					cout << ":" <<  i << ":" << q[i] << ":" << q_dot[i] << ":" << tau[i];
				}

			}
			if ((simtime - old_time) > 0) {
				cout << endl;
			}


			old_time = simtime;

			// Set joint torques
			probot->SetDOFTorques(tau, false);

/*

			// Get Joint position and velocities
			probot->GetDOFValues(q);
			probot->GetDOFVelocities(q_dot);

//cout << "Joint: 0 pos: " << q[0] << endl;
//cout << "Joint: 0 vel: " << q_dot[0] << endl;



// torque = -Kv(vel - velDes) - Kp(pos - posDes)

	       	float simtime = penv->GetSimulationTime()*1e-6;
			if ((simtime - old_time) > 0) {
        		//cout << "sim time: " << simtime << ": ";
				cout << simtime;
			}	



			dReal Kp = 100.0;
			dReal Kv = 60.0;		//2*sqrt(Kp);
			dReal Ki = 0.02;
			for (int i=0; i<3; i++) {

				pTerm = Kp*(q[i] - q_d[i]);
				dTerm = Kv*(q_dot[i] - q_dot_d[i]);
				iTerm[i] += (q[i] - q_d[i]) * Ki;

				tau[i] = -dTerm - pTerm - iTerm[i];

//				tau[i] = -Kv * (q_dot[i] - q_dot_d[i]) - Kp*(q[i] - q_d[i]);
				if ((simtime - old_time) > 0) {
					//cout << "Joint:" << i << ":Pos:" << q[i] << ":Vel:" << q_dot[i] << ":Torque:" << tau[i] << ":";	//<< endl;
					cout << ":" <<  i << ":" << q[i] << ":" << q_dot[i] << ":" << tau[i];
				}

			}
			if ((simtime - old_time) > 0) {
				cout << endl;
			}


			old_time = simtime;

			// Set joint torques
			probot->SetDOFTorques(tau, false);

*/


                }


		}


    }
};

} // end namespace cppexamples

int main(int argc, char ** argv)
{
    cppexamples::ShowSensorsExample example;
    return example.main(argc,argv);
}







